// Do not remove the include below
#include "arduino-canbus-firmware.h"

// tasks for aha card
// read can message and load fromCanQueue
void readCanTask() {
	Can::CAN_MSG canMsg;
	if (Can.readMessage(&canMsg))
		fromCanQueue.push(canMsg);
}

// process can message
void readFromCanQueueTask() {
	Can::CAN_MSG canMsg;
	if (fromCanQueue.count() > 0) {
		canMsg = fromCanQueue.pop();
		Aha.processInputMessage(&canMsg);
	}
}

void ahaTask() {
	Aha.process();
}
// read toCanQueue for sending message to CAN
void readToCanQueueTask() {
	Can::CAN_MSG canMsg;
	if (toCanQueue.count() > 0) {
		canMsg = toCanQueue.peek();
		if (Can.sendMessage(&canMsg))
			toCanQueue.pop();
	}
}


//The setup function is called once at startup of the sketch
void setup()
{
// Add your initialization code here
	// Initialize SPI protocol whith MCP2515
	Spi.init();
	// Initialize AHA card
	Aha.init();
	// initialise CAN MCP2515
	Can.init();
	Can.setMode(Can::_OPERATION_);
}

// The loop function is called in an endless loop
void loop()
{
//Add your repeated code here
	readCanTask();
	readFromCanQueueTask();
	readToCanQueueTask();
	ahaTask();
}
